#include "atos_power_toggle.h"
#include <atos_strings.h>
#include <osa_task.h>

bool atos_power_toggle(atos_uart_fd_t fd, const atos_power_toggle_params_t *params, bool poweron)
{
#ifdef _UCOS
	static int16_t i;
	static uint16_t j;
	static uint16_t sz;
	static bool need_toggle;
	int8_t matched;
	params->toggle(fd); /* Toggle the power first */
    
    osa_task_sleep(1000); /* wait a second before doing anything to the hardware */

	/* We toggle the power at the very first time, so the
	 * restart is usable */
	matched = 0;
	i = 0;
	for (; i < params->try_count; ++i)
	{
		for (j = 0; j < 3; ++j)
		{
			if (osa_sem_wait(fd->sem, 100) != OSA_SUCCESS)
			{
				break;
			}
		}

		atos_fifo_clear(&(fd->fifo));
		atos_uart_send_data(fd, params->at_cmd, params->at_cmd_len);
		osa_sem_wait(fd->sem, params->wait_data_time);
		osa_task_sleep(100);
		sz = atos_fifo_size(&(fd->fifo));
		for (j = 0; j < sz; ++j)
		{
			if (params->at_ret_len
					== mc_common_prefix_len((const char*)(fd->fifo.buf + j), sz - j, params->at_ret))
			{
				break;
			}
		}

		if (j < sz)
		{
			matched = matched >= 0 ? matched + 1 : matched + 2;
			osa_task_sleep(500);
		}
		else /* Didn't match the ret */
		{
			matched = matched <= 0 ? matched - 1 : matched - 2;
		}


		need_toggle = false;
		/* We reached the needed status */
		if (poweron)
		{
			if (matched >= params->matched) /* Powered on. */
			{
				break;
			}
			else if (matched <= params->missed)
			{
				need_toggle = true;
			}
		}
		else
		{
			if (matched <= params->missed) /* Powered off. */
			{
				break;
			}
			else if (matched >= params->matched)
			{
				need_toggle = true;
			}
		}
		if (need_toggle)
		{
			matched = 0;
			params->toggle(fd);
		}
	}
	atos_fifo_clear(&(fd->fifo));

	if (matched > 0)
	{
		return true;
	}

	return false;
#else
	return true;
#endif
}
